{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ConstantVelocityFactor\n",
    "\n",
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/ConstantVelocityFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
    "\n",
    "## Overview\n",
    "\n",
    "The `ConstantVelocityFactor` (contributed by [Asa Hammond](https://www.linkedin.com/in/asahammond/) in 2021) is a simple motion model factor that connects two `NavState` variables at different times, $t_i$ and $t_j$. It enforces the assumption that the velocity remained constant between the two time steps.\n",
    "\n",
    "Given `NavState` $X_i$ (containing pose $T_i$ and velocity $v_i$) and `NavState` $X_j$ (containing pose $T_j$ and velocity $v_j$), and the time difference $\\Delta t = t_j - t_i$, this factor penalizes deviations from the constant velocity prediction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [],
   "source": [
    "%pip install --quiet gtsam-develop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Mathematical Formulation\n",
    "\n",
    "The factor uses the `NavState::update` method (or equivalent logic internally) to predict the state at time $t_j$ based on the state at $t_i$ and the assumption of constant velocity (and zero acceleration/angular velocity). Let this prediction be $X_{j, pred}$:\n",
    "$$ X_{j, pred} = X_i . \\text{update}(\\text{accel}=0, \\text{omega}=0, \\Delta t) $$ \n",
    "Essentially, this integrates the velocity $v_i$ over $\\Delta t$ to predict the change in position, while keeping orientation and velocity constant:\n",
    "$$ R_{j, pred} = R_i $$ \n",
    "$$ v_{j, pred} = v_i $$ \n",
    "$$ p_{j, pred} = p_i + R_i (v_i^{body}) \\Delta t \\quad \\text{(using body velocity update)} $$ \n",
    "\n",
    "The factor's 9-dimensional error $e$ is the difference between the predicted state $X_{j, pred}$ and the actual state $X_j$, expressed in the tangent space at $X_{j,pred}$:\n",
    "$$ e = \\text{localCoordinates}_{X_{j, pred}}(X_j) $$ \n",
    "The noise model associated with the factor determines how strongly deviations from this constant velocity prediction are penalized."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Key Functionality / API\n",
    "\n",
    "- **Constructor**: `ConstantVelocityFactor(key1, key2, dt, model)`: Creates the factor connecting the `NavState` at `key1` (time $i$) and `key2` (time $j$), given the time difference `dt` and a 9D noise model.\n",
    "- **`evaluateError(state1, state2)`**: Calculates the 9D error vector based on the constant velocity prediction from `state1` to `state2` over the stored `dt`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Usage Example\n",
    "\n",
    "This factor is often used as a simple process model between consecutive states when higher-fidelity IMU integration is not available or needed."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Created ConstantVelocityFactor:\n",
      "  keys = { x0 x1 }\n",
      "  noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.1; 0.1; 0.1; 0.05; 0.05; 0.05];\n",
      "\n",
      "Error for perfect prediction (should be zero): [0. 0. 0. 0. 0. 0. 0. 0. 0.]\n",
      "Error for velocity change: [0.  0.  0.  0.  0.  0.  0.  0.1 0. ]\n",
      "Error for position change: [0.   0.   0.   0.   0.05 0.   0.   0.   0.  ]\n"
     ]
    }
   ],
   "source": [
    "import gtsam\n",
    "import numpy as np\n",
    "from gtsam.symbol_shorthand import X # Using X for NavState keys here\n",
    "\n",
    "# Define keys for two NavStates\n",
    "key1 = X(0)\n",
    "key2 = X(1)\n",
    "\n",
    "# Time difference between states\n",
    "dt = 0.1 # seconds\n",
    "\n",
    "# Define a noise model - how much deviation from constant velocity is allowed?\n",
    "# Example: Tighter on rotation/velocity, looser on position change due to velocity\n",
    "rot_sigma = 0.01 # rad\n",
    "pos_sigma = 0.1 # m \n",
    "vel_sigma = 0.05 # m/s\n",
    "sigmas = np.concatenate([\n",
    "    np.full(3, rot_sigma), \n",
    "    np.full(3, pos_sigma),\n",
    "    np.full(3, vel_sigma)\n",
    "])\n",
    "noise_model = gtsam.noiseModel.Diagonal.Sigmas(sigmas)\n",
    "\n",
    "# Create the factor\n",
    "cv_factor = gtsam.ConstantVelocityFactor(key1, key2, dt, noise_model)\n",
    "\n",
    "print(\"Created ConstantVelocityFactor:\")\n",
    "cv_factor.print()\n",
    "\n",
    "# --- Example Evaluation ---\n",
    "# State 1: At origin, moving along +X at 1 m/s\n",
    "pose1 = gtsam.Pose3()\n",
    "vel1 = np.array([1.0, 0.0, 0.0])\n",
    "state1 = gtsam.NavState(pose1, vel1)\n",
    "\n",
    "# State 2: Exactly matches constant velocity prediction\n",
    "pose2 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.0, 0.0]))\n",
    "vel2 = vel1 # Constant velocity\n",
    "state2_perfect = gtsam.NavState(pose2, vel2)\n",
    "\n",
    "error_perfect = cv_factor.evaluateError(state1, state2_perfect)\n",
    "print(\"\\nError for perfect prediction (should be zero):\", error_perfect)\n",
    "\n",
    "# State 3: Velocity changed slightly\n",
    "vel3 = np.array([1.0, 0.1, 0.0]) # Added Y velocity\n",
    "state3_vel_change = gtsam.NavState(pose2, vel3) # Keep predicted pose\n",
    "\n",
    "error_vel_change = cv_factor.evaluateError(state1, state3_vel_change)\n",
    "print(\"Error for velocity change:\", error_vel_change)\n",
    "\n",
    "# State 4: Position is slightly off prediction\n",
    "pose4 = gtsam.Pose3(gtsam.Rot3(), np.array([1.0*dt, 0.05, 0.0])) # Y pos is off\n",
    "state4_pos_change = gtsam.NavState(pose4, vel2) # Keep velocity\n",
    "\n",
    "error_pos_change = cv_factor.evaluateError(state1, state4_pos_change)\n",
    "print(\"Error for position change:\", error_pos_change)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Source\n",
    "- [ConstantVelocityFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ConstantVelocityFactor.h)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
